/*
 * Copyright (c) 2015-2019, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <ti/posix/ccs/unistd.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <ti/posix/ccs/unistd.h>
#include <errno.h>

#include <ti/drivers/GPIO.h>

#include "ti_drivers_config.h"
#include "dbger.h"
#include "libmodbus/modbus.h"

#define SERVER_ID       (0x17)
#define ADDRESS_START   (0)
#define ADDRESS_END     (99)
#define LOOP            (100)

extern "C" {
    #if defined(DBG_DISPLAY)
        extern void APP_openDisplay(void);
    #endif
    void *mainThread(void *arg0);
}

/*
 *  ======== mainThread ========
 */
void *mainThread(void *arg0)
{
    modbus_t *ctx;
    int rc, nb, nb_fail, addr, nb_loop;
    uint8_t *tab_rq_bits;
    uint8_t *tab_rp_bits;
    uint16_t *tab_rq_registers;
    uint16_t *tab_rw_rq_registers;
    uint16_t *tab_rp_registers;

    GPIO_init();
    DBG_INIT();
    DBG_INFO("/************* Modbus Program Start **************/");

    GPIO_setConfig(CONFIG_GPIO_LED_0, GPIO_CFG_OUT_STD | GPIO_CFG_OUT_LOW);
    GPIO_write(CONFIG_GPIO_LED_0, CONFIG_GPIO_LED_ON);

    ctx = modbus_new_rtu(UART2_Modbus, 115200, 'N', 8, 1);
    modbus_set_slave(ctx, SERVER_ID);
    modbus_set_debug(ctx, TRUE);

    if(modbus_connect(ctx) == -1) {
        modbus_free(ctx);
        DBG_ERROR("Modbus connect failed!");
        while(1);
    }

    nb = ADDRESS_END - ADDRESS_START;

    tab_rq_bits = (uint8_t *) malloc(nb * sizeof(uint8_t));
    memset(tab_rq_bits, 0, nb * sizeof(uint8_t));

    tab_rp_bits = (uint8_t *) malloc(nb * sizeof(uint8_t));
    memset(tab_rp_bits, 0, nb * sizeof(uint8_t));

    tab_rq_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
    memset(tab_rq_registers, 0, nb * sizeof(uint16_t));

    tab_rp_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
    memset(tab_rp_registers, 0, nb * sizeof(uint16_t));

    tab_rw_rq_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
    memset(tab_rw_rq_registers, 0, nb * sizeof(uint16_t));

    nb_loop = 0;
    while (nb_loop++ < LOOP) {
        nb_fail = 0;
        //for (addr = ADDRESS_START; addr < ADDRESS_END; addr++) {
        addr = ADDRESS_START;

        /* Random numbers (short) */
        for (int i=0; i<nb; i++) {
            tab_rq_registers[i] = (uint16_t) (65535.0*rand() / (RAND_MAX + 1.0));
            tab_rw_rq_registers[i] = ~tab_rq_registers[i];
            tab_rq_bits[i] = tab_rq_registers[i] % 2;
        }
        nb = ADDRESS_END - addr;

        /* WRITE BIT */
        rc = modbus_write_bit(ctx, addr, tab_rq_bits[0]);
        if (rc != 1) {
            DBG_ERROR("ERROR modbus_write_bit (%d)", rc);
            DBG_ERROR("Address = %d, value = %d", addr, tab_rq_bits[0]);
            nb_fail++;
        } else {
            rc = modbus_read_bits(ctx, addr, 1, tab_rp_bits);
            if (rc != 1 || tab_rq_bits[0] != tab_rp_bits[0]) {
                DBG_ERROR("ERROR modbus_read_bits single (%d)\n", rc);
                DBG_ERROR("address = %d\n", addr);
                nb_fail++;
            }
        }

        /* MULTIPLE BITS */
        rc = modbus_write_bits(ctx, addr, nb, tab_rq_bits);
        if (rc != nb) {
            DBG_ERROR("ERROR modbus_write_bits (%d)\n", rc);
            DBG_ERROR("Address = %d, nb = %d\n", addr, nb);
            nb_fail++;
        } else {
            rc = modbus_read_bits(ctx, addr, nb, tab_rp_bits);
            if (rc != nb) {
                DBG_ERROR("ERROR modbus_read_bits\n");
                DBG_ERROR("Address = %d, nb = %d\n", addr, nb);
                nb_fail++;
            } else {
                for (int i=0; i<nb; i++) {
                    if (tab_rp_bits[i] != tab_rq_bits[i]) {
                        DBG_ERROR("ERROR modbus_read_bits\n");
                        DBG_ERROR("Address = %d, value %d (0x%X) != %d (0x%X)\n",
                               addr, tab_rq_bits[i], tab_rq_bits[i],
                               tab_rp_bits[i], tab_rp_bits[i]);
                        nb_fail++;
                    }
                }
            }
        }

        /* SINGLE REGISTER */
        rc = modbus_write_register(ctx, addr, tab_rq_registers[0]);
        if (rc != 1) {
            DBG_ERROR("ERROR modbus_write_register (%d)\n", rc);
            DBG_ERROR("Address = %d, value = %d (0x%X)\n",
                   addr, tab_rq_registers[0], tab_rq_registers[0]);
            nb_fail++;
        } else {
            rc = modbus_read_registers(ctx, addr, 1, tab_rp_registers);
            if (rc != 1) {
                DBG_ERROR("ERROR modbus_read_registers single (%d)\n", rc);
                DBG_ERROR("Address = %d\n", addr);
                nb_fail++;
            } else {
                if (tab_rq_registers[0] != tab_rp_registers[0]) {
                    DBG_ERROR("ERROR modbus_read_registers single\n");
                    DBG_ERROR("Address = %d, value = %d (0x%X) != %d (0x%X)\n",
                           addr, tab_rq_registers[0], tab_rq_registers[0],
                           tab_rp_registers[0], tab_rp_registers[0]);
                    nb_fail++;
                }
            }
        }

        /* MULTIPLE REGISTERS */
        rc = modbus_write_registers(ctx, addr, nb, tab_rq_registers);
        if (rc != nb) {
            DBG_ERROR("ERROR modbus_write_registers (%d)\n", rc);
            DBG_ERROR("Address = %d, nb = %d\n", addr, nb);
            nb_fail++;
        } else {
            rc = modbus_read_registers(ctx, addr, nb, tab_rp_registers);
            if (rc != nb) {
                DBG_ERROR("ERROR modbus_read_registers (%d)\n", rc);
                DBG_ERROR("Address = %d, nb = %d\n", addr, nb);
                nb_fail++;
            } else {
                for (int i=0; i<nb; i++) {
                    if (tab_rq_registers[i] != tab_rp_registers[i]) {
                        DBG_ERROR("ERROR modbus_read_registers\n");
                        DBG_ERROR("Address = %d, value %d (0x%X) != %d (0x%X)\n",
                               addr, tab_rq_registers[i], tab_rq_registers[i],
                               tab_rp_registers[i], tab_rp_registers[i]);
                        nb_fail++;
                    }
                }
            }
        }

        /* R/W MULTIPLE REGISTERS */
        rc = modbus_write_and_read_registers(ctx,
                                             addr, nb, tab_rw_rq_registers,
                                             addr, nb, tab_rp_registers);
        if (rc != nb) {
            DBG_ERROR("ERROR modbus_read_and_write_registers (%d)\n", rc);
            DBG_ERROR("Address = %d, nb = %d\n", addr, nb);
            nb_fail++;
        } else {
            for (int i=0; i<nb; i++) {
                if (tab_rp_registers[i] != tab_rw_rq_registers[i]) {
                    DBG_ERROR("ERROR modbus_read_and_write_registers READ\n");
                    DBG_ERROR("Address = %d, value %d (0x%X) != %d (0x%X)\n",
                           addr, tab_rp_registers[i], tab_rw_rq_registers[i],
                           tab_rp_registers[i], tab_rw_rq_registers[i]);
                    nb_fail++;
                }
            }

            rc = modbus_read_registers(ctx, addr, nb, tab_rp_registers);
            if (rc != nb) {
                DBG_ERROR("ERROR modbus_read_registers (%d)\n", rc);
                DBG_ERROR("Address = %d, nb = %d\n", addr, nb);
                nb_fail++;
            } else {
                for (int i=0; i<nb; i++) {
                    if (tab_rw_rq_registers[i] != tab_rp_registers[i]) {
                        DBG_ERROR("ERROR modbus_read_and_write_registers WRITE\n");
                        DBG_ERROR("Address = %d, value %d (0x%X) != %d (0x%X)\n",
                               addr, tab_rw_rq_registers[i], tab_rw_rq_registers[i],
                               tab_rp_registers[i], tab_rp_registers[i]);
                        nb_fail++;
                    }
                }
            }
        }

        DBG_INFO("Test: ");
        if(nb_fail) {
            DBG_INFO("%d FAILS in nb_loop[%d]", nb_fail, nb_loop);
            break;
        } else {
            DBG_INFO("SUCCESS");
        }
    }

    /* Free the memory */
    free(tab_rq_bits);
    free(tab_rp_bits);
    free(tab_rq_registers);
    free(tab_rp_registers);
    free(tab_rw_rq_registers);

    modbus_close(ctx);
    modbus_free(ctx);
    DBG_INFO("Program finished.");
    return NULL;
}
